//
// Created by 16933 on 2024/1/13.
//

#include "Message.h"
#include "cmsis_os.h"
#include "bsp_OLED.h"
#include <stdlib.h>
#include "Calculate.h"
#include <stdio.h>
#include "bsp_MPU6050.h"
#include "bsp_fifo.h"
#include "usart.h"
#include "stdbool.h"
#include "Control.h"
#include "tim.h"
Message_State message_state;
char mpu_data_show[10] = {};

extern DMA_HandleTypeDef hdma_usart2_rx;
fifo_s_t bluetooth_message_fifo;
uint8_t blutooth_message_fifo_space[FIFO_BUFER_LENGTH];
uint8_t blutooth_DMA_buffer[UART_DMA_RX_BUFFER_LENGTH];
uint8_t k230_data = 2;
Message blutooth_message;
BluetoothParseStep step = START;
bool bluetooth_connected = 0;
uint8_t data_temp[50];
bool parse_flag = 0;
int datasize = 0;
extern Motor_Controller motor;
int pwm;
void MessageShowTask(void const * argument)
{
    /* USER CODE BEGIN MessageShowTask */
    uint32_t Contiol_time = osKernelSysTick();

    fifo_s_init(&bluetooth_message_fifo,blutooth_message_fifo_space,FIFO_BUFER_LENGTH);
    HAL_UARTEx_ReceiveToIdle_DMA(&huart2,blutooth_DMA_buffer,UART_DMA_RX_BUFFER_LENGTH);
    HAL_UART_Receive_DMA(&huart1,&k230_data,1);
    __HAL_DMA_DISABLE_IT(&hdma_usart2_rx,DMA_IT_HT);
    /* Infinite loop */
    for(;;)
    {
        __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,pwm);// 测试舵机用
        if (parse_flag)
        {
            BluetoothParse(&bluetooth_message_fifo);
            parse_flag = 0;
        }
        switch (k230_data) {
            case '0':
                motor.drive_weel.blutooth_flag = 1;
                motor.drive_weel.speed_bluetooth += 30;
//                blutooth_message.forward += 30;
                break;
            case '1':
                motor.drive_weel.blutooth_flag = 0;
                motor.drive_weel.speed_bluetooth = 0;
//                blutooth_message.forward = 0;
//                blutooth_message.backward = 0;
                break;
            case '2':
                motor.drive_weel.blutooth_flag = 1;
//                blutooth_message.forward = 0;
//                blutooth_message.backward = 0;
                motor.drive_weel.speed_bluetooth = 0;
                break;
            case '5':
                motor.drive_weel.blutooth_flag = 1;
//                if (blutooth_message.forward != 0)
//                {
//                    blutooth_message.forward -=30;
//                }
//                blutooth_message.backward += 30;
                motor.drive_weel.speed_bluetooth -= 30;
                break;
        }
        OLED_NewFrame();
        switch (message_state) {
            case READY:
                OLED_PrintASCIIString(10,20,"roll:",&afont16x8,OLED_COLOR_NORMAL);
                dtoa_manual(mpu_data.gyro_data[0],mpu_data_show,3);
                OLED_PrintASCIIString(50,20,mpu_data_show,&afont16x8,OLED_COLOR_NORMAL);
                switch (k230_data) {
                    case '0':
                        OLED_PrintASCIIString(10,40,"k230 0",&afont16x8,OLED_COLOR_NORMAL);
                        break;
                    case '1':
                        OLED_PrintASCIIString(10,40,"k230 1",&afont16x8,OLED_COLOR_NORMAL);
                        break;
                    case '2':
                        OLED_PrintASCIIString(10,40,"k230 2",&afont16x8,OLED_COLOR_NORMAL);
                        break;
                    case '5':
                        OLED_PrintASCIIString(10,40,"k230 5",&afont16x8,OLED_COLOR_NORMAL);
                        break;
                }

                break;

            case MPU_INIT_ERROR:
                OLED_PrintASCIIString(5,25,"mpu init error",&afont16x8,OLED_COLOR_NORMAL);
                break;


        }
        OLED_ShowFrame();

        osDelay(1);
    }
    /* USER CODE END MessageShowTask */
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    if (huart == &huart1)
    {
        HAL_UART_Receive_DMA(&huart1,&k230_data,1);
    }
}

void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    if (huart == &huart2)
    {
        int rx_len = UART_DMA_RX_BUFFER_LENGTH - __HAL_DMA_GET_COUNTER(&hdma_usart2_rx);
        fifo_s_puts(&bluetooth_message_fifo,(char *)blutooth_DMA_buffer,rx_len);
        HAL_UARTEx_ReceiveToIdle_DMA(&huart2,blutooth_DMA_buffer,UART_DMA_RX_BUFFER_LENGTH);
        __HAL_DMA_DISABLE_IT(&hdma_usart2_rx,DMA_IT_HT);
        parse_flag = 1;
    }
}

void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart)
{
    if (huart == &huart1)
    {
        if (__HAL_UART_GET_FLAG(huart,UART_FLAG_FE))
        {
            __HAL_UART_CLEAR_FEFLAG(huart);
            HAL_UART_Receive_DMA(&huart1,&k230_data,1);
        }

    }
    if (huart == &huart2)
    {
        if (__HAL_UART_GET_FLAG(huart,UART_FLAG_FE))
        {
            __HAL_UART_CLEAR_FEFLAG(huart);
            HAL_UARTEx_ReceiveToIdle_DMA(&huart2,blutooth_DMA_buffer,UART_DMA_RX_BUFFER_LENGTH);
            __HAL_DMA_DISABLE_IT(&hdma_usart2_rx,DMA_IT_HT);
        }
    }
}

void BluetoothParse(fifo_s_t *data_fifo)
{
    uint8_t byte_data;

    while (fifo_s_used(data_fifo))
    {
        byte_data = fifo_s_get(data_fifo);
        switch (step)
        {
            case START:
                if (byte_data == '+')
                {
                    step = DATA_INFO_TYPE;
                } else if (byte_data == 0xa5)
                {
                    data_temp[datasize++] = byte_data;
                    step = DATA_CONTROL;
                }
                break;
            case DATA_INFO_TYPE:
                if (byte_data == 'C')
                {
                    step = DATA_INFO_2;
                    bluetooth_connected = true;
                } else if (byte_data == 'D')
                {
                    step = END_INFO;
                    bluetooth_connected = false;
                } else
                {
                    step = START;
                }
                break;
            case DATA_CONTROL:
                if (byte_data != 0x5a && datasize <= UART_DMA_RX_BUFFER_LENGTH)
                {
                    data_temp[datasize++] = byte_data;
//                    step = START;
                } else if (byte_data == 0x5a)
                {
                    data_temp[datasize++] = byte_data;
                    memcpy(&blutooth_message,data_temp,datasize);
                    step = START;
                    datasize = 0;
                } else
                {
                    step = START;
                }
                break;

            case DATA_INFO_2:
                if (byte_data == '\n')
                {
                    step = END_INFO;
                }
                break;
            case END_INFO:
                if (byte_data == '\n')
                {
                    memset(&blutooth_message,0, sizeof(blutooth_message));
                    step = START;
                }
                break;
        }
    }
}





// 将整数部分转换为字符串
void int_to_str(int num, char str[], int* index) {
    if (num == 0) {
        str[(*index)++] = '0';
        return;
    }

    int digits[10]; // 假设最多10位整数部分
    int i = 0;

    // 提取每一位数字
    while (num > 0) {
        digits[i++] = num % 10;
        num /= 10;
    }

    // 反向添加到字符串中
    while (i > 0) {
        str[(*index)++] = digits[--i] + '0';
    }
}

// 将小数部分转换为字符串
void frac_to_str(double frac, char str[], int precision, int* index) {
    int i;
    for (i = 0; i < precision; ++i) {
        frac *= 10;
        int digit = (int)frac;
        str[(*index)++] = digit + '0';
        frac -= digit;
    }
}

// 将小数转换为字符串（完全避免sprintf）
void dtoa_manual(double num, char str[], int precision) {
    int index = 0;

    // 处理负数
    if (num < 0) {
        num = -num;
        str[index++] = '-';
    }

    // 整数部分
    int_to_str((int)num, str, &index);

    // 小数点
    str[index++] = '.';

    // 小数部分
    frac_to_str(num - (int)num, str, precision, &index);

    // 添加字符串结束符
    str[index] = '\0';
}